# -*- coding: utf-8 -*-
"""
Created on Mon Oct  7 17:17:28 2020

@author: Erfan
"""
from __future__ import print_function, division
import numpy as np
import time
from casadi import *
import scipy.linalg
from Parameters import *
from Pre_Jacobian import F1,F2

def discrete_EKF(x_pre,x_cur,u_pre,X_cur,P_pre,Q,R,C,b,a,kc,et):
    
    Nr,Nt,Nz,dr,dt,dz,Np,NN,Nx,Nw,Ny,Nv=circular_parameters()
    DeltaT,Nsim=time_parameters()


        ## Obtaining the continuous time forms of the A and B matrices
    E1=time.time()  
    A=F1(x_pre,u_pre,b,a,kc,et)
    E2=time.time()-E1
    print('The continuious time model takes',E2,' seconds to compute')
    
    
    
        # Conversion from the continuous time model to the discrete time model
    E33=time.time()
    A_c=DeltaT*A
    a_c=np.array(A_c)
    a_d=scipy.linalg.expm(a_c)
    Ad=DM(a_d)
    E44=time.time()-E33
    print('The conversion to discrete time took',E44,'seconds to compute') 

    

        # Updating the covariance matrix based on the model
    E7=time.time()
    p_min=DM((np.mat(Ad)*np.mat(P_pre)*np.mat(Ad.T))+np.mat(Q))
    P_min=DM(p_min)    
    E8=time.time()-E7
    print('The P update based on the model took',E8,'seconds to compute')


    

        # Obtaining the gain matrix
    E9=time.time()
    K_k = mtimes(mtimes(P_min, np.transpose(C)), np.linalg.inv(
                mtimes(mtimes(C, P_min), np.transpose(C)) + R))   
    E10=time.time()-E9
    print('The gain took',E10,'to commpute')
    
    
         ## update current estimate based on measurement x(k|k)
    E11=time.time()
    xekf = x_cur + mtimes(K_k, (X_cur - mtimes(C,x_cur))) 
    E12=time.time()-E11
    print('The measurement update step took',E12,'seconds to compute')
 
    
    
        ## The Measurement Update of the Covariance Matrix
    E13=time.time()
    P_plus=DM((np.mat(DM.eye(Nx))-np.mat(K_k)*np.mat(C))*np.mat(P_min))
    E14=time.time()-E13
    print('The measurement update of P took',E14,'seconds to compute')
    
    
    xekf = xekf.full().ravel()
    P_plus = P_plus.full()
    
    return [xekf, P_plus]
    
